#include <string>
#include <iostream>
#include "utils.hpp"

void fatal_error(std::string msg) {
	std::cout << "Error: " << msg << std::endl;
	exit(EXIT_FAILURE);
}

double randunity() {
	return (double)rand()/RAND_MAX;
}

double fastabs(double x) {
	return (x<0) ? -x : x;
}

double fastsign(double x) {
	return (x<0) ? -1 : 1;
}

double fastmin(double a, double b) {
	return (a<b) ? a : b;
}

double fastmax(double a, double b) {
	return (a>b) ? a : b;
}

bool line_line_intersect(double x1, double y1, double x2, double y2, double u1, double v1, double u2, double v2, double &xint, double &yint, double &t, double &u)
// does [p p+r]=(x1,y1)->(x2,y2) intersect [q q+s]=(u1,v1)->(u2,v2)?  return true if so
// xint and yint record coordinates of intersection if it exists
// t contains fraction along (x1,y1)->(x2,y2) where intersect occurs, and u contains fraction along (u1,v1)->(u2,v2)
{
	// first line extends from vector p to vector p+r
	double px=x1;
	double py=y1;
	double rx=x2-x1;
	double ry=y2-y1;
	// second line extends from vector q to vector q+s
	double qx=u1;
	double qy=v1;
	double sx=u2-u1;
	double sy=v2-v1;
	// find solution of p+t*r=q+u*s by taking cross product with r and s
	double denom=rx*sy-ry*sx;						// r cross s
//	std::cout << x1 << "," << y1 << " " << x2 << "," << y2 << " " << u1 << "," << u2 << " " << v1 << "," << v2 << " " << endl;
	t=(sx*(py-qy)-sy*(px-qx))/denom;			// { s cross (p-q) } / (r cross s)
	u=(rx*(py-qy)-ry*(px-qx))/denom;			// { r cross (p-q) } / (r cross s)
	//
	if (0<=t&&t<=1&&0<=u&&u<=1)	// t and u lie on the unit interval if the two lines intersect
	{
		xint=px+rx*t;	// here is the point of intersection, which is a fraction t along direction r from point p (could also be calculated from q and s)
		yint=py+ry*t;
		return true;	
	}
	return false;
}

double* linspace(double x1, double x2, unsigned npts)
{
	double *x=new double[npts];	// assume npts>0
	for (unsigned k=0; k<npts; k++)
		x[k]=x1+double(k)/double(npts-1)*(x2-x1);
	return x;
}

bool linearIntervalSearch(double *array, int size, double value, unsigned &position) {
	for (int i=0; i<size-1; i++)
		if (value>=array[i] && value<array[i+1]) {
			position=i;
			return true;
		}
	return false;
}

bool binaryIntervalSearch(double *array, int size, double value, unsigned& position)	{	// this was shamefully adapted from a Wikipedia page (http://en.wikibooks.org/wiki/Algorithm_Implementation/Search/Binary_search)
	int low = 0, high = size - 1, midpoint = 0;
	while (low <= high) {
		midpoint = low + (high - low)/2;
		// deal with endpoint (Mike Z)
		if (midpoint==size-1) {
			if (value==array[midpoint]) {
				position=midpoint;
				return true;
			}
			else
				return false;
		}
		// modified what used to be a == condition to check local interval instead of immediate array value (Mike Z)
		if (value>=array[midpoint] && value<array[midpoint+1]) {	
			position = midpoint;
			return true;
		}
		else if (value < array[midpoint])
			high = midpoint - 1;
		else
			low = midpoint + 1;
	}
	return false;
}

